
#include "fuction_temp.hpp"
// #include <pcl/features/normal_3d.h>
// #include <pcl/sample_consensus/ransac.h>
// #include <pcl/search/kdtree.h>
#include <cmath>
#include <transforms3d/transforms3d.h>

inline double my_abs(double a)
{
    if(a>=0)return a;
    else return -a;
}
inline double distance_to_line(pcl::PointXYZ p0,pcl::PointXYZ p1,pcl::PointXYZ p2)
{
      double slope,intercept,numerator,denominator;
     if (0.01<my_abs(p0.x-p1.x)) { // 如果直线垂直于x轴，则特殊处理
        return my_abs(p2.y - p0.y);
    }
    if (0.01<my_abs(p0.y-p1.y)) { // 如果直线垂直于x轴，则特殊处理
        return my_abs(p2.x - p0.x);
    }
    if(my_abs(p0.x-p1.x)>my_abs(p0.y-p1.y))
    {
         slope = (p1.y-p0.y) / (p1.x-p0.x); // 斜率
         intercept = p0.y - slope * p0.x; // 截距

         numerator = my_abs(slope * (p2.x - p0.x) + intercept - p2.y); // 点到线距离公式分子
         denominator = sqrt(std::pow(slope, 2) + 1); // 根据斜率计算距离分母
    }
    else
    {
       slope = (p1.x-p0.x) / (p1.y-p0.y); // 斜率
         intercept = p0.x - slope * p0.y; // 截距

        numerator = my_abs(slope * (p2.y - p0.y) + intercept - p2.x); // 点到线距离公式分子
         denominator = sqrt(std::pow(slope, 2) + 1); // 根据斜率计算距离分母
    }
    return numerator / denominator;
}
//找出直线
void move_single_point(pcl::PointCloud<pcl::PointXYZ> &cloud,pcl::PointCloud<pcl::PointXYZ> &dscloud)
{
  std::size_t size;
  size = cloud.points.size();
  double dis,lim_dis;
  std::set<int> index;
  for(int i=0;i<size-20;i++)
    {
        int flag=0,j;
    //    for(int j=1;j<19;j++) 
    //    {
        dis=distance_to_line(cloud.points[i],cloud.points[i+10],cloud.points[i+5]);
          lim_dis = 0.010*sqrt((cloud.points[i+5].x*cloud.points[i+5].x)+(cloud.points[i+5].y*cloud.points[i+5].y));
          if(dis<lim_dis)index.insert(i+5);
    }
    pcl::PointXYZ pp;
    std::vector<int>vec(index.begin(),index.end());
    std::sort(vec.begin(),vec.end());
  size=vec.size();
  for(int i=0;i<size;i++)
  {
    pp=cloud.points[vec[i]];
    pp.z=0.2;
    dscloud.points.push_back(pp);
  }  
}
//void copy_point(pcl::PointCloud<pcl::PointXYZ> &cloud,pcl::PointCloud<pcl::PointXYZ> &dscloud)
////去掉车体中心轴点
void move_stable_point(pcl::PointCloud<pcl::PointXYZ> &cloud,pcl::PointCloud<pcl::PointXYZ> &dscloud)
{
  std::size_t size;
  size = cloud.points.size();
  for(int i=0;i<size;i++)
  {
    if(!(((cloud.points[i].x<0)&&(cloud.points[i].x>-1.5))&&((cloud.points[i].y>0)&&(cloud.points[i].y<1.5))))
           dscloud.points.push_back(cloud.points[i]);
  }
}
void seat_transform(pcl::PointXYZ seat,double z_rotate,
                pcl::PointCloud<pcl::PointXYZ> &cloud,
                pcl::PointCloud<pcl::PointXYZ> &dscloud)
   {
      // Vector3d positon = {seat.x,seat.y,seat.z};
      // Quaterniond quat=TransForms::Euler2Quat(0, 0, z_rotate);
      // Matrix4d matr=TransForms::Compose(positon,quat);
      Matrix4d matr=TransForms::ComposeEuler(seat.x,seat.y,seat.z,0,0,z_rotate);
      pcl::transformPointCloud(cloud,dscloud,matr);
   }
